#include "usr_inc.h"

extern ChassisState_t chassisState;

extern GM6020_t gm1;
extern GM6020_t gm2;
extern GM6020_t gm3;
extern GM6020_t gm4;

/**
 * @brief 初始化所有CAN控制器
 */
void ALL_BSP_CAN_Init(void)
{
    FDCAN_FilterTypeDef canFliter = {0};

    canFliter.IdType = FDCAN_STANDARD_ID;
    canFliter.FilterIndex = 0;
    canFliter.FilterType = FDCAN_FILTER_MASK;
    canFliter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
    canFliter.FilterID1 = 0x0000;
    canFliter.FilterID2 = 0x0000;
    HAL_FDCAN_ConfigFilter(&hfdcan1, &canFliter);
    HAL_FDCAN_ConfigFilter(&hfdcan2, &canFliter);
    HAL_FDCAN_ConfigFilter(&hfdcan3, &canFliter);

    /* 开启FDCAN外设 */
    HAL_FDCAN_Start(&hfdcan1); // FDCAN外设的开启需要在中断的使能之前
    HAL_FDCAN_Start(&hfdcan2);
    HAL_FDCAN_Start(&hfdcan3);

    /* 使能中断 */
    HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
    HAL_FDCAN_ActivateNotification(&hfdcan2, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
    HAL_FDCAN_ActivateNotification(&hfdcan3, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);

    /* 过滤所有不匹配帧 */
    HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE);
    HAL_FDCAN_ConfigGlobalFilter(&hfdcan2, FDCAN_REJECT, FDCAN_REJECT, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE);
    HAL_FDCAN_ConfigGlobalFilter(&hfdcan3, FDCAN_REJECT, FDCAN_REJECT, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE);
}

/**
 * @brief 重定义RxFifo0的中断回调函数
 */
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if ((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET)
    {
        if (hfdcan->Instance == FDCAN1)
        {
            FDCAN_RxHeaderTypeDef rxHeader;
            uint8_t rxData[8];
            HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rxHeader, rxData);

            if (rxHeader.Identifier - 0x204 == gm1.motorID)
            {
                ISR_GM6020_UpdateInfo(rxData, &gm1, gm1InfoBoxHandle);
            }
            else if (rxHeader.Identifier - 0x204 == gm2.motorID)
            {
                ISR_GM6020_UpdateInfo(rxData, &gm2, gm2InfoBoxHandle);
            }
            else if (rxHeader.Identifier >> 6 != 0x00 && (rxHeader.Identifier & 0x3F) == DGM_CMD_STATUSWORD_REPORT)
            {
                DgmStatuswordMsg_t msg;
                msg.id = rxHeader.Identifier >> 6;
                memcpy(&(msg.state), rxData + 0, sizeof(uint32_t));
                memcpy(&(msg.error), rxData + 4, sizeof(uint32_t));
                xQueueSendToBackFromISR(dgmStatuswordMsgQueueHandle, &msg, NULL);
            }
        }
        else if (hfdcan->Instance == FDCAN2)
        {
            FDCAN_RxHeaderTypeDef rxHeader;
            uint8_t rxData[8];
            HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rxHeader, rxData);

            if (rxHeader.Identifier - 0x204 == gm3.motorID)
            {
                ISR_GM6020_UpdateInfo(rxData, &gm3, gm3InfoBoxHandle);
            }
            else if (rxHeader.Identifier - 0x204 == gm4.motorID)
            {
                ISR_GM6020_UpdateInfo(rxData, &gm4, gm4InfoBoxHandle);
            }
            else if (rxHeader.Identifier >> 6 != 0x00 && (rxHeader.Identifier & 0x3F) == DGM_CMD_STATUSWORD_REPORT)
            {
                DgmStatuswordMsg_t msg;
                msg.id = rxHeader.Identifier >> 6;
                memcpy(&(msg.state), rxData + 0, sizeof(uint32_t));
                memcpy(&(msg.error), rxData + 4, sizeof(uint32_t));
                xQueueSendToBackFromISR(dgmStatuswordMsgQueueHandle, &msg, NULL);
            }
        }
        else if (hfdcan->Instance == FDCAN3)
        {
            FDCAN_RxHeaderTypeDef rxHeader;
            uint8_t rxData[16];
            HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rxHeader, rxData);

            if (rxHeader.Identifier == 0x100)
            {
                ChassisSpdCmdMsg_t msg;
                memcpy(&(msg.vX), rxData + 0, sizeof(float));
                memcpy(&(msg.vY), rxData + 4, sizeof(float));
                memcpy(&(msg.w), rxData + 8, sizeof(float));

                xQueueSendToBackFromISR(spdCmdQueueHandle, &msg, NULL);
            }
            else if (rxHeader.Identifier == 0x160)
            {
                chassisState = CHASSIS_FREE;
            }
            else if (rxHeader.Identifier == 0x161)
            {
                chassisState = CHASSIS_LOCK;
            }
        }
    }
}
